%% Runge Kutta and Second Order ODE as a vector of First Order ODE


%This is largely taken from Feb_22.m

T=20;
Dt=.1;

%This is the spring equation
k=1;
m=1;

%Here is our update of the 2-element vector [X V]
Xp=@(X)[X(2),-(k/m)*X(1)]

%Setup
T0=0;
Tvec=(T0:Dt:T)
X=zeros(length(Tvec),2);
X0=10;
V0=0;
X(1,1)=X0;
X(1,2)=V0;

%This is the integration.  
%see: web('https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods')

for tt=2:length(Tvec)
    %Calculate slopes for the vector
    k1=Xp(X(tt-1,:));
    k2=Xp( X(tt-1,:)+k1*Dt/2 );
    k3=Xp( X(tt-1,:)+k2*Dt/2 );
    k4=Xp( X(tt-1,:)+k3*Dt );
    %Update based on weighted average of slopes
    X(tt,:)=X(tt-1,:)+ 1/6*(k1+2*k2+2*k3+k4)*Dt;  
end

figure
plot(Tvec,X(:,1)) %X(:,1) is our array of X positions
xlabel('t')
ylabel('X(t)')


%% Central Potential 

T=10;
Dt=.1;

% x'' = -A / r^3 * X
% Where X is a vector [x y]

% Convention for our 2D set of first order equations
% [X Y Vx Vy]

%Take a careful look at the equation and make sure you understand it. 
A=-100
Xp=@(X)[X(3),X(4),A*X(1)/(sqrt(X(1)^2+X(2)^2))^3,A*X(2)/(sqrt(X(1)^2+X(2)^2))^3 ]

%Setup
T0=0;
Tvec=(T0:Dt:T);
X=zeros(length(Tvec),4);

%Initial Positions and Velocities
X(1,1)=10;
X(1,2)=0;
X(1,3)=0;
X(1,4)=2;

%RK4 method
for tt=2:length(Tvec)
    k1=Xp(X(tt-1,:));
    k2=Xp( X(tt-1,:)+k1*Dt/2 );
    k3=Xp( X(tt-1,:)+k2*Dt/2 );
    k4=Xp( X(tt-1,:)+k3*Dt );
    X(tt,:)=X(tt-1,:)+ 1/6*(k1+2*k2+2*k3+k4)*Dt;  
end

figure
plot(X(:,1),X(:,2)) %Plotting X vs Y
xlabel('X')
ylabel('Y')

%figure
hold on
quiver(X(:,1),X(:,2),X(:,3),X(:,4),0) %plot velocity vector

%% Showing some dynamics

figure

for tt=1:length(Tvec)
    plot(X(tt,1),X(tt,2),'bo') %Plotting X vs Y
    hold on
    plot(0,0,'ro')
    quiver(X(tt,1),X(tt,2),X(tt,3),X(tt,4),0,'g')
    hold off
    axis([2*min(X(:,1)) 2*max(X(:,1)) 2*min(X(:,2)) 2*max(X(:,2)) ]);
    xlabel('X')
    ylabel('Y')
    pause(.1)
end

















